/*
 * ekf.cpp
 *
 *  Created on: 2021年4月17日
 *      Author: 17900
 */

//说明 该状态估计参考PX4-ECL
//系统总共13个状态变量 四元数 北东地速度 北东地位置 角度增量的偏置


#include "ekf.hpp"

void EKF::ekfInit()
{
	//一些变量初始化
	dt = 0.005;				//EKF执行周期
	gravity = 9.788f;		//当地的重力加速度
	osDelay(180);			//延时以等待传感器初始化完成

	//求初始状态变量和协方差矩阵
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueMagDat, &mag, 0);					//从队列中获取磁力计数据
	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取水平位置数据
	xQueuePeek(queueHeight, &height, 0);				//从队列中获取高度数据

	//通过加速度计和磁力计求初始姿态角 并转成四元数
	MatAcc << -acc.acc[0],-acc.acc[1],-acc.acc[2];			//Unit:m/s2
	MatMag << mag.mag[0],mag.mag[1],mag.mag[2];			//Unit:uT

	//Acc、Mag数据归一化
	MatAcc.normalize();
	MatMag.normalize();

	Euler[0] = atan2f(MatAcc(1),MatAcc(2));			//初始横滚角
	Euler[1] = -asinf(MatAcc(0));					//初始俯仰角

	float mag_calib[2];
	float sinR,cosR,sinP,cosP;
	sinR = sinf(Euler[0]);
	cosR = cosf(Euler[0]);
	sinP = sinf(Euler[1]);
	cosP = cosf(Euler[1]);

	mag_calib[0]=-MatMag(1)*cosR + MatMag(2)*sinR;//分子
	mag_calib[1]=MatMag(0)*cosP + MatMag(1)*sinP*sinR + MatMag(2)*sinP*cosR;//分母

	if (mag_calib[1] != 0)
	{
		Euler[2] = atan2f(mag_calib[0], mag_calib[1]);		//初始航向角
	}
	else
	{
		if (mag_calib[0] < 0)
			Euler[2] = -M_PI/2;
		else
			Euler[2] = M_PI/2;
	}

	//欧拉角转四元数
	q_p = AngleAxisf(Euler(2),Vector3f::UnitZ())*AngleAxisf(Euler(1),Vector3f::UnitY())*AngleAxisf(Euler(0),Vector3f::UnitX());
	q_p.normalize();				//单位化四元数
	X_vel_p << 0.0f,0.0f,0.0f;		//初始北东地速度
	X_pos_p << uwbPos.uwbPos[0],uwbPos.uwbPos[1],-height.height;		//初始北东地位置

	//初始状态
	X_p << q_p.w(),q_p.x(),q_p.y(),q_p.z(),X_vel_p[0],X_vel_p[1],X_vel_p[2],X_pos_p[0],X_pos_p[1],X_pos_p[2];

	//初始协方差矩阵
	P_p = 1e-4f*MatrixXf::Identity(10, 10);

	//先将F阵的对角线元素初始化，其他元素在迭代过程中会进行更新
	F = MatrixXf::Identity(10, 10);

	//北东地位置更新灵敏度矩阵
	H_uwb << 0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,
			 0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,
			 0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f;

	float GYRO_NOISE;
	float GYRO_NOISE2;
	float ACC_NOISE;
	GYRO_NOISE = 0.01f;			//陀螺仪噪声
	GYRO_NOISE2 = 0.03f;			//陀螺仪噪声
	ACC_NOISE = 0.1f;				//加速度计噪声
	float DELTA_ANGLE_NOISE;
	float DELTA_ANGLE_NOISE2;
	float DELTA_VEL_NOISE;
	DELTA_ANGLE_NOISE = (GYRO_NOISE*dt)*(GYRO_NOISE*dt);
	DELTA_ANGLE_NOISE2 = (GYRO_NOISE2*dt)*(GYRO_NOISE2*dt);
	DELTA_VEL_NOISE = (ACC_NOISE*dt)*(ACC_NOISE*dt);
	//输入噪声
	Q << DELTA_ANGLE_NOISE,0.0f,0.0f,0.0f,0.0f,0.0f,
	     0.0f,DELTA_ANGLE_NOISE,0.0f,0.0f,0.0f,0.0f,
		 0.0f,0.0f,DELTA_ANGLE_NOISE2,0.0f,0.0f,0.0f,
		 0.0f,0.0f,0.0f,DELTA_VEL_NOISE,0.0f,0.0f,
		 0.0f,0.0f,0.0f,0.0f,DELTA_VEL_NOISE,0.0f,
		 0.0f,0.0f,0.0f,0.0f,0.0f,DELTA_VEL_NOISE;

	N_process.Zero();

	//北东地位置测量噪声 UWB的标准差为0.02m
	R_uwb << 0.09f,0.0f,0.0f,
			 0.0f,0.09f,0.0f,
			 0.0f,0.0f,0.00025f;

	//航向角的测量噪声
	R_yaw = 0.0001f;		//4°标准差
}

void EKF::ekfRun()
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据
	xQueuePeek(queueDeltaImu, &delta_angle_and_vel, 0);
	xQueuePeek(queueDeltaImuToZeroFlag, &delta_to_zeros_flag, 0);
	xQueuePeek(queueHeight, &height, 0);
	xQueuePeek(queueMagDat, &mag, 0);					//从队列中获取磁力计数据

	//将角度速度增量置零标志位置1
	*(delta_to_zeros_flag.delta_m_to_zeros_flag) = 1;

	//EKF迭代过程
	//UWB更新过程
	//UWB测量值,Z轴用激光高度
	z_uwb << uwbPos.uwbPos[0],uwbPos.uwbPos[1],-height.height;
	//卡尔曼增益
	K_uwb = P_p*H_uwb.transpose()*(H_uwb*P_p*H_uwb.transpose() + R_uwb).inverse();
	//位置预测值
	z_uwb_p = X_pos_p;
	//状态更新
	X_k = X_p + K_uwb * (z_uwb - z_uwb_p);
	//协方差更新
	P_k = (MatrixXf::Identity(10, 10) - K_uwb * H_uwb) * P_p;



	//由于磁力计的更新频率慢 所以有新的磁力计数据才进行航向更新 通过时间戳来判断磁力计数据是否更新
	static uint32_t magLastUpdateTimeUs = mag.timestamp;
	if(mag.timestamp > magLastUpdateTimeUs)
	{
		magLastUpdateTimeUs = mag.timestamp;

		//通过磁力计数据求航向角测量值
		//更新后的状态变量
		q_k.w() = X_k(0);
		q_k.x() = X_k(1);
		q_k.y() = X_k(2);
		q_k.z() = X_k(3);
		q_k.normalize();		//每次四元数预测更新后都需要进行单位化
		MatMag << mag.mag[0],mag.mag[1],mag.mag[2];
		MatMag.normalize();
		float roll,pitch;
		roll = atan2f(2.0f*(q_k.w()*q_k.x()+q_k.y()*q_k.z()),1.0f-2.0f*(q_k.x()*q_k.x()+q_k.y()*q_k.y()));
		pitch = asinf(fConstrain(-2.0f * q_k.x() * q_k.z() + 2.0f * q_k.w() * q_k.y(),-1.0f,1.0f));
		R_tilt = AngleAxisf(pitch,Vector3f::UnitY())*AngleAxisf(roll,Vector3f::UnitX());
		Vector3f mag_tilt;
		mag_tilt = R_tilt * MatMag;
		z_yaw = -atan2f(mag_tilt(1),mag_tilt(0));
		//灵敏度矩阵
		float H_q;
		float H_q1;
		float H_Denominator;
		H_q = q_k.w()*q_k.w() + q_k.x()*q_k.x() - q_k.y()*q_k.y() - q_k.z()*q_k.z();
		H_q1 = 2.0f*q_k.w()*q_k.z() + 2.0f*q_k.x()*q_k.y();
		H_Denominator = H_q1*H_q1/(H_q*H_q) + 1.0f;
		H_yaw << (2.0f*q_k.z()/H_q - 2.0f*q_k.w()*H_q1/(H_q*H_q))/H_Denominator,
				 (2.0f*q_k.y()/H_q - 2.0f*q_k.x()*H_q1/(H_q*H_q))/H_Denominator,
				 (2.0f*q_k.x()/H_q + 2.0f*q_k.y()*H_q1/(H_q*H_q))/H_Denominator,
				 (2.0f*q_k.w()/H_q + 2.0f*q_k.z()*H_q1/(H_q*H_q))/H_Denominator,
				 0.0f,0.0f,0.0f,0.0f,0.0f,0.0f;

		//卡尔曼增益
		Matrix<float, 1, 1> Hyaw_mul_Pk_mul_HyawT;
		Hyaw_mul_Pk_mul_HyawT = H_yaw*P_k*H_yaw.transpose();
		K_yaw = P_k * H_yaw.transpose()/(Hyaw_mul_Pk_mul_HyawT(0,0) + R_yaw);
		//航向角预测值
		z_yaw_p = atan2f(2.0f*(q_k.w()*q_k.z()+q_k.x()*q_k.y()),1.0f-2.0f*(q_k.y()*q_k.y()+q_k.z()*q_k.z()));
		z_yaw_sub_z_yaw_p = z_yaw - z_yaw_p;
		if(z_yaw_sub_z_yaw_p > M_PI)
			z_yaw_sub_z_yaw_p = z_yaw_sub_z_yaw_p - 2.0f*M_PI;
		else if(z_yaw_sub_z_yaw_p < -M_PI)
			z_yaw_sub_z_yaw_p = z_yaw_sub_z_yaw_p + 2.0f*M_PI;
		//状态更新
		X_k = X_k + K_yaw*z_yaw_sub_z_yaw_p;
		//协方差更新
		P_k = (MatrixXf::Identity(10, 10) - K_yaw * H_yaw) * P_k;

	}

	//更新后的状态变量
	q_k.w() = X_k(0);
	q_k.x() = X_k(1);
	q_k.y() = X_k(2);
	q_k.z() = X_k(3);
	q_k.normalize();		//每次四元数预测更新后都需要进行单位化
	Cb2n = q_k.toRotationMatrix();
	X_vel_k << X_k(4),X_k(5),X_k(6);
	X_pos_k << X_k(7),X_k(8),X_k(9);

	//状态变量的预测
	//四元数预测
	Vector3f delta_angle_m;
	delta_angle_m << delta_angle_and_vel.delta_ang_m[0],delta_angle_and_vel.delta_ang_m[1],delta_angle_and_vel.delta_ang_m[2];

	q_p = q_k * AngleAxisf(delta_angle_m(2),Vector3f::UnitZ())*AngleAxisf(delta_angle_m(1),Vector3f::UnitY())*AngleAxisf(delta_angle_m(0),Vector3f::UnitX());
	q_p.normalize();	//每次四元数预测更新后都需要进行单位化

	//速度预测
	Vector3f gNED;
	gNED << 0.0f,0.0f,gravity;
	Vector3f delta_vel_m;
	delta_vel_m << delta_angle_and_vel.delta_vel_m[0],delta_angle_and_vel.delta_vel_m[1],delta_angle_and_vel.delta_vel_m[2];
	X_vel_p = X_vel_k + Cb2n * delta_vel_m + gNED * dt;

	//位置预测
	X_pos_p = X_pos_k + X_vel_k * dt;

	X_p << q_p.w(),q_p.x(),q_p.y(),q_p.z(),X_vel_p[0],X_vel_p[1],X_vel_p[2],X_pos_p[0],X_pos_p[1],X_pos_p[2];

	//协方差预测
	Matrix<float, 4, 4> F_q_q;
	Matrix<float, 3, 4> F_v_q;
	Matrix<float, 3, 3> F_p_v;
	F_q_q << 1.0f,                -delta_angle_m(0)/2.0f,-delta_angle_m(1)/2.0f,-delta_angle_m(2)/2.0f,
            delta_angle_m(0)/2.0f, 1.0f,                  delta_angle_m(2)/2.0f,-delta_angle_m(1)/2.0f,
            delta_angle_m(1)/2.0f,-delta_angle_m(2)/2.0f, 1.0f,                  delta_angle_m(0)/2.0f,
            delta_angle_m(2)/2.0f, delta_angle_m(1)/2.0f,-delta_angle_m(0)/2.0f, 1.0f;

    F_v_q << 2.0f*(q_k.w()*delta_vel_m(0)-q_k.z()*delta_vel_m(1)+q_k.y()*delta_vel_m(2)), 2.0f*(q_k.x()*delta_vel_m(0)+q_k.y()*delta_vel_m(1)+q_k.z()*delta_vel_m(2)), 2.0f*(-q_k.y()*delta_vel_m(0)+q_k.x()*delta_vel_m(1)+q_k.w()*delta_vel_m(2)), 2.0f*(-q_k.z()*delta_vel_m(0)-q_k.w()*delta_vel_m(1)+q_k.x()*delta_vel_m(2)),
    		 2.0f*(q_k.z()*delta_vel_m(0)+q_k.w()*delta_vel_m(1)-q_k.x()*delta_vel_m(2)), 2.0f*(q_k.y()*delta_vel_m(0)-q_k.x()*delta_vel_m(1)-q_k.w()*delta_vel_m(2)), 2.0f*(q_k.x()*delta_vel_m(0)+q_k.y()*delta_vel_m(1)+q_k.z()*delta_vel_m(2)), 2.0f*(q_k.w()*delta_vel_m(0)-q_k.z()*delta_vel_m(1)+q_k.y()*delta_vel_m(2)),
			 2.0f*(-q_k.y()*delta_vel_m(0)+q_k.x()*delta_vel_m(1)+q_k.w()*delta_vel_m(2)), 2.0f*(q_k.z()*delta_vel_m(0)+q_k.w()*delta_vel_m(1)-q_k.x()*delta_vel_m(2)), 2.0f*(-q_k.w()*delta_vel_m(0)+q_k.z()*delta_vel_m(1)-q_k.y()*delta_vel_m(2)), 2.0f*(q_k.x()*delta_vel_m(0)+q_k.y()*delta_vel_m(1)+q_k.z()*delta_vel_m(2));


    F_p_v = dt * MatrixXf::Identity(3, 3);

	F.block(0, 0, 4, 4) = F_q_q;
	F.block(4, 0, 3, 4) = F_v_q;
	F.block(7, 4, 3, 3) = F_p_v;

	Matrix<float, 4, 3> G_q_delta_angle_m;
	Matrix<float, 3, 3> G_v_delta_v_m;
	G_q_delta_angle_m << -q_k.x()/2,-q_k.y()/2,-q_k.z()/2,
			              q_k.w()/2,-q_k.z()/2, q_k.y()/2,
						  q_k.z()/2, q_k.w()/2,-q_k.x()/2,
						 -q_k.y()/2, q_k.x()/2, q_k.w()/2;
	G_v_delta_v_m = Cb2n;

	G.block(0, 0, 4, 3) = G_q_delta_angle_m;
	G.block(4, 3, 3, 3) = G_v_delta_v_m;

	P_p = F*P_k*F.transpose() + G*Q*G.transpose();

	//将解算得到的状态发送到队列中
	for(uint8_t i = 0; i < 3; i++)
	{
		ekf.pos[i] = X_pos_k(i);
		ekf.vel[i] = X_vel_k(i);
	}
	ekf.q[0] = q_k.w();
	ekf.q[1] = q_k.x();
	ekf.q[2] = q_k.y();
	ekf.q[3] = q_k.z();

	xQueueOverwrite(queueEKF,&ekf);

	getTimer_us(&stopTimer);
	executionTime_us = stopTimer - startTimer;

}

EKF ekf((char *)"EKF_ECL");
extern "C" void ekf_main(void *argument)
{
	ekf.ekfInit();
	osDelay(200);
	for(;;)
	{
		osSemaphoreAcquire(semEkf,0xffffffff);
		ekf.ekfRun();
	}
}



